package com.rockwell.robot2015.model.sensors;

import com.rockwell.robot2015.modbus.ModbusSerialClient;

import net.wimpi.modbus.ModbusException;
import net.wimpi.modbus.ModbusIOException;
import net.wimpi.modbus.ModbusSlaveException;

public class RobotSensors implements IRobotSensors {
	private static final int UNIT_ID = 1;	// unit id of the modbus slave
	
	private float orientation = 0.0f;

	ModbusSerialClient m_modbusMaster;

	private LeftAngle m_leftAngle = new LeftAngle();
	
	public RobotSensors(ModbusSerialClient modbusMaster) {
		m_modbusMaster = modbusMaster;
	}
	
	@Override
	public boolean hasLeftDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public boolean hasFrontDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public boolean hasRightDistanceSensor() {
		return true;	// should be read from settings
	}
	
	@Override
	public double getLeftDistance() {
		try {
			return readInt(LEFT_DISTANCE_SENSOR_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}
	
	@Override
	public boolean detectFrontWall() {
		return getFrontStatus();
	}
	
	@Override
	public boolean getFrontStatus() {
		return readSensorStatus(SENSORS_STATUS_REF, FRONT_SENSOR_MASK);
	}
	
	@Override
	public boolean detectLeftWall() {
		return readSensorStatus(SENSORS_STATUS_REF, LEFT_SENSOR_MASK);
	}

	@Override
	public boolean detectRightWall() {
		return readSensorStatus(SENSORS_STATUS_REF, RIGHT_SENSOR_MASK);
	}
	
	@Override
	public boolean isArmUp() {
		return readSensorStatus(SENSORS_STATUS_REF, ARM_UP_SENSOR_MASK);
	}
	
	@Override
	public boolean isArmDown() {
		return readSensorStatus(SENSORS_STATUS_REF, ARM_DOWN_SENSOR_MASK);
	}
	
	@Override
	public boolean isRightTooClose() {
		return readSensorStatus(SENSORS_STATUS_REF, RIGHT_TOO_CLOSE_MASK);
	}

	@Override
	public boolean isLeftFrontTooClose() {
		return readSensorStatus(SENSORS_STATUS_REF, LEFT_FRONT_TOO_CLOSE_MASK);
	}

	@Override
	public boolean isLeftBackTooClose() {
		return readSensorStatus(SENSORS_STATUS_REF, LEFT_BACK_TOO_CLOSE_MASK);
	}

	@Override
	public boolean isFrontTooClose() {
		return readSensorStatus(SENSORS_STATUS_REF, FRONT_TOO_CLOSE_MASK);
	}

	private boolean readSensorStatus(int ref, short mask) {
		try {
			int read = readInt(ref) & mask;
			return (read != 0);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return false;
		}
	}
	
	@Override
	public double getRightDistance() {
		try {
			return readInt(RIGHT_DISTANCE_SENSOR_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}
	
	@Override
	public float getOrientation() {
		return orientation;
	}
	
	@Override
	synchronized public void setOrientation(float angle) {
		if (Math.abs(orientation - angle) >= 0.5)
		{
			orientation = angle;
		}
	}
	
	private int readInt(int ref) throws ModbusException {
		int v = m_modbusMaster.readHoldingRegister(ref);
		return v;
	}

	private void writeInt(int ref, int value) throws ModbusIOException, ModbusSlaveException, ModbusException {
		m_modbusMaster.writeHoldingRegister(ref, value);
	}
	
	@Override
	public int[] getAllSensorStatus() {
		int[] values = null;
		try {
			values = m_modbusMaster.readHoldingRegisters(1, SENSOR_COUNT);
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return values;
	}
	
	@Override
	public LeftAngle getLeftAngle() {
		try {
			int angle = readInt(ANGLE_SENSORS_REF);
			
			m_leftAngle.direction = (((int)angle) & LEFT_ANGLE_DIRECTION) != 0;
			m_leftAngle.leftBackOpen = (((int)angle) & LEFT_BACK_SENSOR_MASK) != 0;
			m_leftAngle.leftFrontOpen = (((int)angle) & LEFT_FRONT_SENSOR_MASK) != 0;
			m_leftAngle.angle = ((int)angle & LEFT_ANGLE_SENSOR_MASK) / 100;
		}
		catch (ModbusException e) {
			m_leftAngle.leftBackOpen = false;
			m_leftAngle.leftFrontOpen = false;
			m_leftAngle.angle = 0;
		}

		return m_leftAngle;
	}

	/*
	private float getFloatValue(int ref) {
		try {
			int i1 = readInt(ref);
			int i2 = readInt(ref + 1);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return Float.NaN;
		}
	}
	*/
	
	@Override
	public int getLeftTargetValue() {
		try {
			return readInt(LEFT_TARGET_VALUE_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}

	@Override
	public int getLeftPresentValue() {
		try {
			return readInt(LEFT_PRESENT_VALUE_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}

	@Override
	public int getRightTargetValue() {
		try {
			return readInt(RIGHT_TARGET_VALUE_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}

	@Override
	public int getRightPresentValue() {
		try {
			return readInt(RIGHT_PRESENT_VALUE_REF);
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return 0;
		}
	}

	@Override
	public void setInMaze(boolean in) {
		try {
			int value = 0;
			
			if (in) {
				value |= IN_MAZE_MASK;
			}
			
			writeInt(IN_MAZE_REF, value);
		} catch (ModbusIOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusSlaveException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (ModbusException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
}
